#include <stdio.h>
#include <stdint.h>
#include <string.h>

#define HI_NIBBLE(b) (((b) >> 4) & 0x0F)
#define LO_NIBBLE(b) ((b) & 0x0F)


unsigned char digits[] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','*','#'};


uint16_t Fletcher16( uint8_t* data, int count )
  
{
  
  uint16_t sum1 = 0;  
  uint16_t sum2 = 0;
  int index;
  
  
  for( index = 0; index < count; ++index ) {  
    sum1 = (sum1 + data[index]) % 255;
    sum2 = (sum2 + sum1) % 255;
  }
  
  
  return (sum2 << 8) | sum1;
  
}


void checkdigits(uint8_t* data, int count, uint8_t *c0, uint8_t *c1)
{

  uint16_t csum;
  uint8_t f0,f1;
  
  csum = Fletcher16(data, count);
  f0 = csum & 0xff;
  f1 = (csum >> 8) & 0xff;
  *c0 = (f0 + (0xff - f1)) % 0xff;
  *c1 = ((0xff - *c0) + (0xff - f0)) % 0xff;     
}

// simple 1 byte checksum
static unsigned char checksum(unsigned char *ptr, size_t sz) {
  unsigned char chk = 0;
  while (sz-- != 0)
    chk -= *ptr++;
  return chk;
}


size_t encapsulate_data(unsigned char data[], size_t bytes)
{
  uint8_t c0, c1;
  
  // add checkdigits
  checkdigits(data, bytes, &c0, &c1);
  memcpy(data+bytes, &c0, 1);
  memcpy(data+bytes+1, &c1, 1);
  bytes =  bytes + 2;
  
  return bytes;
}

size_t unencapsulate_data(unsigned char data[], size_t bytes)
{
  uint8_t c0, c1;

  checkdigits(data, bytes-2, &c0, &c1);
  if ( (c0 == data[bytes-2]) && (c1 == data[bytes-1]) ) {
    return bytes-2;
  }

  // if checksum failed
  return 0;
}

size_t data_to_dtmfdigits(unsigned char tones[], unsigned char data[], size_t bytes)
{
  int i,j;
  size_t numtones;

  for (i=0,j=0; i<bytes; i+=1,j+=2) {
    tones[j] =  digits[HI_NIBBLE(data[i])];
    tones[j+1] = digits[LO_NIBBLE(data[i])];
    //printf("l:%c,h%c\n", tones[j], tones[j+1]);
  }
  numtones = 2*bytes; 
  tones[numtones] = '\0';

  return numtones;
}


size_t dtmfdigits_to_data(unsigned char data[], unsigned char tones[], size_t digits)
{
  int i,j;
  size_t bytes = 0;
  unsigned char c;

  // get the correct hex values back
  for (i=0; i<digits; i++) {
    c = tones[i];  
    if (c >= '0' && c <= '9') tones[i] = c-48;
    else if (c >= 'A' && c <= 'D') tones[i] = c-55;
    else if (c == '*') tones[i] = 14;
    else if (c == '#') tones[i] = 15;
  }

  // join the nibbles
  for (i=0,j=0; i<digits; i+=2, j+=1, bytes++)
    data[j] = (tones[i]<<4) | (tones[i+1]);

  // for string testing
  data[bytes] = '\0';
  
  return bytes;
}



/* int main() */
/* { */
/*   unsigned char tones[255]; */
/*   unsigned char data[255]; */
/*   size_t numtones, bytes; */
  
/*   numtones = data_to_dtmfdigits(tones, (unsigned char *)"111111", 6); */
/*   printf("tones:%s\n", tones); */
/*   bytes = dtmfdigits_to_data(data, tones, numtones); */
/*   printf("bytes:%d\n", bytes); */
/*   if (bytes) { */
/*     printf("data:%s\n", data); */
/*   } else { */
/*     printf("failed to decode BUFFER\n"); */
/*   } */
  

/*   return 0; */
/* } */
